#include "robolite.h"

//RoboLite motor control functions implementation file

#define LEFT_MOTION_FORWARD 0b01000000
#define LEFT_MOTION_BACKWARD 0b10000000
#define RIGHT_MOTION_FORWARD 0b00100000
#define RIGHT_MOTION_BACKWARD 0b00010000

//software method to PWM
#if USE_SOFT_PWM||FULL_CODE

void rl_move(int L,int R,uint T)
{
    #warning "rl_move compiled."
    uchar motion_left;
    uchar motion_right;

    motion_left=(L>0)?LEFT_MOTION_FORWARD:LEFT_MOTION_BACKWARD;
    motion_right=(R>0)?RIGHT_MOTION_FORWARD:RIGHT_MOTION_BACKWARD;


    while(T--)
    {
        PORTD=motion_left;
        rl_delay_us(L);
        PORTD=motion_right;
        rl_delay_us(R);
    }

}

#endif

#if USE_PWM||FULL_CODE

void rl_motor_ex(int left_speed,int right_speed)
{
    #warning "rl_motor_ex compiled."
    char l,r;
    PORTD=0x00;

    if(left_speed==0&&right_speed==0)
    {
        TCCR0B=0;  //stop timer
        TCCR0A=0;
        return;
    }

    if(left_speed<0)
    {
        rl_setBit(PORTD,4);
        l=left_speed+255;
    }else{
        l=left_speed;
    }

    if(right_speed<0)
    {
        rl_setBit(PORTD,7);
        r=right_speed+255;
    }else{
        r=right_speed;

    }

    OCR0A=r;
    OCR0B=l;

    TCCR0B=1;  //start timer
}


void rl_motor(uchar left_speed,uchar right_speed)
{
    #warning "rl_motor compiled."
    PORTD=0x00;
    if(left_speed==0&&right_speed==0)
    {
        TCCR0B=0;  //stop timer
        TCCR0A=0;
        return;
    }
    OCR0A=right_speed;
    OCR0B=left_speed;

    TCCR0B=1;  //start timer
}

#endif
